
// --- Includes --------------------------------------------------------------

#include "matrix.h"

#define PI 3.1415926535897932384626433832795f

// ---
Matrix4 MultiplyMatrix(const Matrix4 &A, const Matrix4 &B)
{
	Matrix4 C;

	for (int j = 0; j < 4; j++)
	{
		for (int i = 0; i < 4; i++)
		{
			C.m[j][i] = 0;
			for (int k = 0; k < 4; k++)
			{
				C.m[j][i] += A.m[k][i] * B.m[j][k];
			}
			/*C.m[j][i] = A.m[0][i] * B.m[j][0];
			C.m[j][i] += A.m[1][i] * B.m[j][1];
			C.m[j][i] += A.m[2][i] * B.m[j][2];
			C.m[j][i] += A.m[3][i] * B.m[j][3];*/
		}
	}

	return C;
}
// ---

Matrix4 MultiplyMatrix(const Matrix4 */* __restrict*/ A, const Matrix4 * /*__restrict*/ B)
{
	Matrix4 C;

	for (int j = 0; j < 4; j++)
	{
		for (int i = 0; i < 4; i++)
		{
			C.m[j][i] = 0;
			for (int k = 0; k < 4; k++)
			{
				C.m[j][i] += A->m[k][i] * B->m[j][k];
			}
			/*C.m[j][i] = A.m[0][i] * B.m[j][0];
			C.m[j][i] += A.m[1][i] * B.m[j][1];
			C.m[j][i] += A.m[2][i] * B.m[j][2];
			C.m[j][i] += A.m[3][i] * B.m[j][3];*/
		}
	}

	return C;
}


Matrix4 RotateX(float angle)
{
	angle = angle * PI / 180.f;
	Matrix4 m;
	m.Identity();
	float c = cosf(angle), s = sinf(angle);
	m.Y.y = c; m.Y.z = s;
	m.Z.y = -s; m.Z.z = c;
	return m;
}

Matrix4 RotateY(float angle)
{
	angle = angle * PI / 180.f;
	Matrix4 m;
	m.Identity();
	float c = cosf(angle), s = sinf(angle);
	m.X.x = c; m.X.z = -s;
	m.Z.x = s; m.Z.z = c;
	return m;
}

Matrix4 RotateZ(float angle)
{
	angle = angle * PI / 180.f;
	Matrix4 m;
	m.Identity();
	float c = cosf(angle), s = sinf(angle);
	m.X.x = c; m.X.y = s;
	m.Y.x = -s; m.Y.y = c;
	return m;
}

Matrix4 Translate(float x, float y, float z)
{

	Matrix4 m;
    m.Identity();

    m.X.z += x;
	m.Y.z += y;
	m.Z.z += z;

	return m;
    //*this = *this * translation;
}


// camera ---

Matrix4 LookAt(const Vector3 &eye, const Vector3 &target, const Vector3 &up)
{
	Vector3 z = (target - eye).NormalizedSafe();
	if (z.Length() == 0.f)
		z.z = -1.f;
	Vector3 y = up.Normalized();
	Vector3 x = (z.Cross(y)).Normalized();
	y = x.Cross(z);
	Matrix4 m;
    m.X = Vector4(x, 0);
    m.Y = Vector4(y, 0);
    m.Z = Vector4(-z, 0);
	m.W = Vector4(0,0,0,1);
	m.Transpose();
    Vector4 eyePrime = m*Vector4(-eye, 1);	
    m.W = eyePrime;
	return m;
}

// projection ---
Matrix4 Ortho(float left, float right, float bottom, float top, float near, float far)
{
	float a = 2.0f / (right - left);
	float b = 2.0f / (top - bottom);
	float c = -2.0f / (far - near);
	float tx = (right + left) / (right - left);
	float ty = (top + bottom) / (top - bottom);
	float tz = (far + near) / (far - near);

	Matrix4 m;
	m.X.x = a; m.X.y = 0; m.X.z = 0; m.X.w = 0;
	m.Y.x = 0; m.Y.y = b; m.Y.z = 0; m.Y.w = 0;
	m.Z.x = 0; m.Z.y = 0; m.Z.z = c; m.Z.w = 0;
	m.W.x = -tx; m.W.y = -ty; m.W.z = -tz; m.W.w = 1;
	return m;
}

Matrix4 Frustum(float left, float right, float bottom, float top, float near, float far)
{
	float a = 2.f * near / (right - left);
	float b = 2.f * near / (top - bottom);
	float c = (right + left) / (right - left);
	float d = (top + bottom) / (top - bottom);
	float e = (-(far + near)) / (far - near);
	float f = -((2.f * far * near) / (far - near));
	Matrix4 m;
	m.X.x = a; m.X.y = 0; m.X.z = 0; m.X.w = 0;
	m.Y.x = 0; m.Y.y = b; m.Y.z = 0; m.Y.w = 0;
	m.Z.x = c; m.Z.y = d; m.Z.z = e; m.Z.w = -1;
	m.W.x = 0; m.W.y = 0; m.W.z = f; m.W.w = 0;
	return m;
}

Matrix4 Perspective(float fovy, float aspect, float near, float far)
{
	float frustumW, frustumH;
   
	frustumH = std::tanf( fovy / 360.0f * PI ) * near;
	frustumW = frustumH * aspect;

	return Frustum(-frustumW, frustumW, -frustumH, frustumH, near, far);
}
